
#include <asm.h>

.global C_SYMBOL(RGB24Blit2RGB24)

C_SYMBOL(RGB24Blit2RGB24):

pSrc = 0x0;
srcX = 0x4;
srcY = 0x8;
width = 0xC;
height = 0x10;
srcPitch = 0x14;
srcOpacity = 0x18;

    MOV     R12, SP
    STMFD   SP!, {R4-R12,LR}

    MUL     R4, R3, R2
    ADD     R1, R4, R1, LSL#2
    ADD     R2, R0, R1

    LDR     R4, [R12,#srcPitch]
    LDR     R5, [R12,#srcY]
    LDR     R6, [R12,#srcX]
    LDR     R8, [R12,#pSrc]
    MUL     R7, R4, R5
    ADD     R6, R7, R6, LSL#2
    ADD     R0, R8, R6
    MOV     R1, R4

    LDR     R4, [R12,#width]
    LDR     R5, [R12,#height]
    LDR     R8, [R12,#srcOpacity]

    AND     R8, R8, #0xFF
    CMP     R8, #255

    BLT     _Start_T_
    MOV R4, R4, LSL #2

    SUB	R1, R1,	R4                          //R1 = SRC pitch - ROI width
    SUB	R3, R3,	R4                          //R3 = DST pitch - ROI width

_Line_Start_:
    MOV	R6, R4                              //R6 = ROI width
    ADD	R7, R2,	#7                          //R2 = destination pointer
    BIC	R7, R7,	#7                          //Ceiling R7 ADDRESS to 8 byte aligned, suppose jumped N
    SUB	R7, R7,	R2                          //
    CMP	R7, #0                              //Compare to see if R7 is originally 8 byte aligned
    BEQ	_Loop_Aligned8_Head_                //if so jump to _Loop_Aligned8_Head_

_Less_Than_32_:
    WLDRW   wR0 ,[R0], #4                   //R0 = Source pointer, LOAD 4 byte

    CMP	R7, R6                              //
    MOVGT	R7, R6                          //Let R7 be the min(N , width)
    SUB	R6, R6,	R7                          //Let R6 be width - N

_Process_Step_N_header_:                    //:SAVE N byte
    SUBS	R7, R7,	#4                      //N--, modify CPSR
    WSTRW   wR0 ,[R2], #4                   //SAVE 4 byte
    WLDRWGT   wR0 ,[R0], #4                 //if width > N, LOAD 1 byte again

    BGT	_Process_Step_N_header_                      //if width > N, jump to _Process_Step_N_header_
    CMP	R6, #0
    BEQ	_Next_Line_                         //if R6 = width - N == 0 jump to _Next_Line_

_Loop_Aligned8_Head_:
    SUBS	R6, R6,	#0x20                   //R6 -= 32 byte
    ADDMI	R7, R6,	#0x20
    ADDMI	R6, R6,	#0x20
    BMI	_Less_Than_32_                      //jumpt to _Less_Than_32_ if less than 32 byte
    TST	R0, #7
    BNE	_UnAligned8_                        //if pSrc is not 8 byte aligned jump to _UnAligned8_

    WLDRD   wR0, [R0], #8
    WLDRD   wR1, [R0], #8

_Loop_Aligned8_:
    PLD	[R0,#0x40]
//    PLD	[R2,#0x40]

    WSTRD   wR0, [R2], #8
    WLDRD   wR2, [R0], #8
    WSTRD   wR1, [R2], #8
    WLDRD   wR3, [R0], #8

    SUBS	R6, R6,	#0x20

    WSTRD   wR2, [R2], #8
    WLDRDGE wR0, [R0], #8
    WSTRD   wR3, [R2], #8
    WLDRDGE wR1, [R0], #8

    BGE	_Loop_Aligned8_

    ADDS	R6, R6,	#0x20
    BEQ	_Next_Line_
    MOVGT	R7, R6
    BGT	_Less_Than_32_

_UnAligned8_:
    AND	R7, R0,	#7
    BIC	R0, R0,	#7
    TMCR    wCGR0, R7
    WLDRD   wR0, [R0], #8
    WLDRD   wR1, [R0], #8
    WLDRD   wR2, [R0], #8
    WLDRD   wR3, [R0], #8

_Loop_UnAligned8_:
    SUBS	R6, R6,	#0x20
    PLD	[R0,#0x40]
//    PLD	[R2,#0x40]
    WLDRD   wR4, [R0]

    WALIGNR0 wR0 ,wR0, wR1
    WSTRD   wR0, [R2], #8
    WLDRDGE wR0, [R0], #8

    WALIGNR0 wR1 ,wR1, wR2
    WSTRD   wR1, [R2], #8
    WLDRDGE wR1, [R0], #8

    WALIGNR0 wR2 ,wR2, wR3
    WSTRD   wR2, [R2], #8
    WLDRDGE wR2, [R0], #8

    WALIGNR0 wR3, wR3, wR4
    WSTRD   wR3, [R2], #8
    WLDRDGE wR3, [R0], #8

    BGE	_Loop_UnAligned8_
    ADD	R0, R0,	R7
    ADDS	R6, R6,	#0x20
    BEQ	_Next_Line_
    MOVGT	R7, R6
    BGT	_Less_Than_32_

_Next_Line_:
    ADD	R0, R0,	R1
    ADD	R2, R2,	R3
    SUBS	R5, R5,	#1
    BGT	_Line_Start_
//.long 0xe7fddefe
    LDMFD   SP, {R4-R11,SP,PC}

_Start_T_:
    CMP   R8, #0
    LDMEQFD   SP, {R4-R11,SP,PC}

    MOV R9, R8
    MOV R11, R0
    MOV R8,  R1
    MOV R10, R2
    MOV R12, R3

    MOV R7, R5

    ORR R9, R9, R9, LSL #16
    TMCRR   wR14, R9, R9

	MOV LR, R4, LSL #2

    LDR     R4, = 0x00FF00FF
    TMCRR   wR15, R4, R4

    LDR     R4, = 0x00000008
    TMCR    wCGR3, R4

    WSUBH   wR15, wR15, wR14

    MOV     R9, #0xFF000000
    TMCRR   wR13, R9, R9

    MOV     R9, #0xFF

_LoopLine_T_:
	MOV	R3, LR
	MOV	R2, R10
	MOV	R0, R11
	B _Alpha_8u_C4ARow_T_
_Next_T_:
	SUB	R7, R7,	#1
	ADD	R11, R11, R8
	ADD	R10, R10, R12
	CMP	R7, #0
	BHI	_LoopLine_T_

_End_T_:

	LDMFD   SP, {R4-R11,SP,PC}

_Alpha_8u_C4ARow_T_:

    ADD	R6, R2,	#7
    BIC	R6, R6,	#7
    SUB	R6, R6,	R2
    CMP	R6, #0
    BEQ	_Aligned8_T_

_Process_One_T_:

    WLDRW   wR4, [R0], #4
    CMP	R6, R3
    WLDRW   wR6, [R2]

    MOVGT	R6, R3
    SUB	R3, R3,	R6

_Loop_One_T_:
//.long 0xe7fddefe
    WUNPCKELUB wR0, wR4
    WMULUL  wR0, wR0, wR14
    WUNPCKELUB wR1, wR6

    WSRLHG    wR0, wR0, wCGR3
    SUBS	R6, R6,	#4
    WMULUL  wR1, wR1, wR15
    WLDRWGT   wR6, [R2, #4]
    WSRLHG    wR1, wR1, wCGR3
    WADDH     wR0, wR0, wR1
    WLDRWGT   wR4, [R0], #4
    WPACKHUS wR0, wR0, wR1

    WSTRW   wR0, [R2], #4

    BGT	_Loop_One_T_

    CMP	R3, #0
    BEQ     _Next_T_

_Aligned8_T_:
    SUBS	R3, R3,	#0x10
    ADDMI	R6, R3,	#0x10
    ADDMI	R3, R3,	#0x10
    BMI	_Process_One_T_
    AND	R4, R0,	#7
    CMP R4, #0
    BNE	_UnAligned8_T_

    WLDRD   wR0, [R0], #8
    WLDRD   wR4, [R2]

    WLDRD   wR2, [R0], #8
    WLDRD   wR5, [R2, #8]

_Loop_Aligned8_T_:

    PLD	[R0,#0x30]
    PLD	[R2,#0x30]
    SUBS	R3, R3,	#0x10

    WUNPCKELUB wR6, wR0
    WMULUL  wR6, wR6, wR14
    WUNPCKELUB wR8, wR4
    WSRLHG  wR6, wR6, wCGR3
    WMULUL  wR8, wR8, wR15
    WUNPCKEHUB wR9, wR0
    WSRLHG  wR8, wR8, wCGR3
    WADDH     wR6, wR6, wR8

    WMULUL  wR9, wR9, wR14
    WUNPCKEHUB wR11, wR4
    WLDRDGE   wR4, [R2, #16]
    WSRLHG  wR9, wR9, wCGR3
    WMULUL  wR11, wR11, wR15
    WLDRDGE wR0, [R0], #8
    WSRLHG  wR11, wR11, wCGR3
    WADDH   wR9, wR9, wR11
    WPACKHUS wR6, wR6, wR9

    WSTRD   wR6, [R2], #8

    WUNPCKELUB wR6, wR2
    WMULUL  wR6, wR6, wR14
    WUNPCKELUB wR8, wR5
    WSRLHG  wR6, wR6, wCGR3
    WMULUL  wR8, wR8, wR15
    WUNPCKEHUB wR9, wR2
    WSRLHG  wR8, wR8, wCGR3
    WADDH     wR6, wR6, wR8

    WMULUL  wR9, wR9, wR14
    WUNPCKEHUB wR11, wR5
    WLDRDGE wR5, [R2, #16]
    WSRLHG  wR9, wR9, wCGR3
    WMULUL  wR11, wR11, wR15
    WLDRDGE wR2, [R0], #8
    WSRLHG  wR11, wR11, wCGR3
    WADDH     wR9, wR9, wR11
    WPACKHUS wR6, wR6, wR9

    WSTRD wR6, [R2], #8

    BGE	_Loop_Aligned8_T_
    ADDS	R3, R3,	#0x10
    BEQ     _Next_T_
    MOVGT	R6, R3
    //SUBGT   R2, R2, #8
    BGT	_Process_One_T_

_UnAligned8_T_:
    BIC	R0, R0,	#7
    TMCR    wCGR1, R4

    WLDRD   wR0, [R0], #8
    WLDRD   wR4, [R2]

    WLDRD   wR2, [R0], #8
    WLDRD   wR5, [R2, #8]

_Loop_UnAligned8_T_:
@.long 0xe7fddefe
    PLD	[R0,#0x30]
    PLD	[R2,#0x30]
    TST	R4, #7
    WLDRDNE wR12, [R0]
    SUBS	R3, R3,	#0x10

    WALIGNR1 wR0 ,wR0, wR2

    WUNPCKELUB wR6, wR0
    WMULUL  wR6, wR6, wR14
    WUNPCKELUB wR8, wR4
    WSRLHG  wR6, wR6, wCGR3
    WMULUL  wR8, wR8, wR15
    WUNPCKEHUB wR9, wR0
    WSRLHG  wR8, wR8, wCGR3
    WADDH     wR6, wR6, wR8

    WMULUL  wR9, wR9, wR14
    WUNPCKEHUB wR11, wR4
    WLDRDGE   wR4, [R2, #16]
    WSRLHG  wR9, wR9, wCGR3
    WMULUL  wR11, wR11, wR15
    WLDRDGE wR0, [R0], #8
    WSRLHG  wR11, wR11, wCGR3
    WADDH   wR9, wR9, wR11
    WPACKHUS wR6, wR6, wR9

    WSTRD   wR6, [R2], #8

    WALIGNR1 wR2 ,wR2, wR12

    WUNPCKELUB wR6, wR2
    WMULUL  wR6, wR6, wR14
    WUNPCKELUB wR8, wR5
    WSRLHG  wR6, wR6, wCGR3
    WMULUL  wR8, wR8, wR15
    WUNPCKEHUB wR9, wR2
    WSRLHG  wR8, wR8, wCGR3
    WADDH     wR6, wR6, wR8

    WMULUL  wR9, wR9, wR14
    WUNPCKEHUB wR11, wR5
    WLDRDGE wR5, [R2, #16]
    WSRLHG  wR9, wR9, wCGR3
    WMULUL  wR11, wR11, wR15
    WLDRDGE wR2, [R0], #8
    WSRLHG  wR11, wR11, wCGR3
    WADDH     wR9, wR9, wR11
    WPACKHUS wR6, wR6, wR9

    WSTRD wR6, [R2], #8

    BGE	_Loop_UnAligned8_T_

    ADD	R0, R0,	R4
    ADDS	R3, R3,	#0x10
    BEQ     _Next_T_
    MOVGT	R6, R3
    BGT	_Process_One_T_
